#include "uavmath.h"

const vector3_t VECTOR3_UNIT_X = { { FIXED_RESOLUTION }, { 0LL },              { 0LL } };
const vector3_t VECTOR3_UNIT_Y = { { 0LL },              { FIXED_RESOLUTION }, { 0LL } };
const vector3_t VECTOR3_UNIT_Z = { { 0LL },              { 0LL },              { FIXED_RESOLUTION } };
const vector3_t VECTOR3_ZERO   = { { 0LL },              { 0LL },              { 0LL } };
const vector3_t VECTOR3_ONE    = { { FIXED_RESOLUTION }, { FIXED_RESOLUTION }, { FIXED_RESOLUTION } };

void vector3_add(const vector3_t *a, const vector3_t *b, vector3_t *out)
{
	out->x.val = a->x.val + b->x.val;
	out->y.val = a->y.val + b->y.val;
	out->z.val = a->z.val + b->z.val;
}

void vector3_sub(const vector3_t *a, const vector3_t *b, vector3_t *out)
{
	out->x.val = a->x.val - b->x.val;
	out->y.val = a->y.val - b->y.val;
	out->z.val = a->z.val - b->z.val;
}

fixed_t vector3_dot(const vector3_t *a, const vector3_t *b)
{
	fixed_t res = fixed_mul(a->x, b->x);
	res.val += fixed_mul(a->y, b->y).val + fixed_mul(a->z, b->z).val;
	return res;
}

void vector3_cross(const vector3_t *a, const vector3_t *b, vector3_t *out)
{
	out->x.val = fixed_mul(a->y, b->z).val - fixed_mul(a->z, b->y).val;
	out->y.val = fixed_mul(a->z, b->x).val - fixed_mul(a->x, b->z).val;
	out->z.val = fixed_mul(a->x, b->y).val - fixed_mul(a->y, b->x).val;
}

fixed_t vector3_length(const vector3_t *a)
{
	fixed_t res = fixed_mul(a->x, a->x);
	res.val += fixed_mul(a->y, a->y).val + fixed_mul(a->z, a->z).val;
	return fixed_sqrt(res);
}

fixed_t vector3_length_squared(const vector3_t *a)
{
	fixed_t res = fixed_mul(a->x, a->x);
	res.val += fixed_mul(a->y, a->y).val + fixed_mul(a->z, a->z).val;
	return res;
}

void vector3_normalize(const vector3_t *a, vector3_t *out)
{
	fixed_t len = vector3_length(a);
	out->x = fixed_div(a->x, len);
	out->y = fixed_div(a->y, len);
	out->z = fixed_div(a->z, len);
}

void vector3_transform(const matrix_t *A, const vector3_t *x, vector3_t *out)
{
	out->x = fixed_mul(A->m11, x->x);
	out->x.val += fixed_mul(A->m12, x->y).val + fixed_mul(A->m13, x->z).val;
	
	out->y = fixed_mul(A->m21, x->x);
	out->y.val += fixed_mul(A->m22, x->y).val + fixed_mul(A->m23, x->z).val;
	
	out->z = fixed_mul(A->m31, x->x);
	out->z.val += fixed_mul(A->m32, x->y).val + fixed_mul(A->m33, x->z).val;
}
